//
// Created by bobin on 18-6-25.
//

#include <fstream>
#include <iomanip>
#include "ros/Path.h"
#include "opencv2/core/core.hpp"
#include <string>

#include <dpptam/superpixel.h>
#include <dpptam/DenseMapping.h>
#include <dpptam/SemiDenseTracking.h>
#include <dpptam/SemiDenseMapping.h>
#include "dpptam/Viewer.h"
#include <thread>
using namespace std;

bool LoadImages(const string &strPathLeft, const string &strPathTimes,
                vector<string> &vstrImageLeft, vector<double> &vTimeStamps);

int main(int argc, char **argv) {
    ///vo_system launch the three threads, tracking, semidense mapping and dense mapping (3D superpixels)
    string config_file = ros::package::getPath("dpptam") + "/src/data-tum.yml";
    cv::FileStorage fs2(config_file, cv::FileStorage::READ);

    DenseMapping *dense_mapper= new DenseMapping(fs2);
    MapShared *Map = new MapShared;

    SemiDenseMapping *semidense_mapper = new SemiDenseMapping(fs2, dense_mapper, Map);
    SemiDenseTracking *semidense_tracker = new SemiDenseTracking(fs2, semidense_mapper);
    semidense_mapper->set_tracker(semidense_tracker);
    Imagenes images, images_previous_keyframe;

    int cont_frames;
    double stamps;
    cv::Mat image_frame, image_frame_aux;


    cont_frames = 0;

    int calculate_superpixels = (int) fs2["calculate_superpixels"];

    vector<string> vstrImage;
    vector<double> vTimeStamp;


    if (!fs2.isOpened()) {
        cout << "ERROR: Wrong path to settings" << endl;
        return -1;
    }
    string leftFolder = fs2["LeftFolder"];
    string timeFolder = fs2["TimeFolder"];

    LoadImages(leftFolder, timeFolder, vstrImage, vTimeStamp);

    if (vstrImage.empty()) {
        cerr << "ERROR: No images in provided path." << endl;
        return 1;
    }

    ros::Time current_time, stamps_ros;
    semidense_tracker->cont_frames = &cont_frames;
    semidense_tracker->stamps = &stamps;
    semidense_tracker->image_frame = &image_frame_aux;
    semidense_tracker->stamps_ros = &stamps_ros;
    ros::Publisher odom_pub;

    ros::Publisher pub_cloud;
    ros::Publisher pub_poses;
    ros::Publisher vis_pub;
    image_transport::Publisher pub_image;
    ///Launch semidense tracker thread*/
    boost::thread thread_semidense_tracker(&SemiDenseTracking::Run, semidense_tracker, &images,
                                           dense_mapper, Map, &odom_pub, &pub_poses, &vis_pub, &pub_image);

    ///Launch semidense mapper thread
    boost::thread thread_semidense_mapper(&SemiDenseMapping::run, semidense_mapper, &images, &images_previous_keyframe, &pub_cloud);


    if (calculate_superpixels > 0.5) {
        ///launch dense mapper thread
        boost::thread thread_dense_mapper(&ThreadDenseMapper, dense_mapper, &pub_cloud);
    }


    const int nImages = vstrImage.size();
    for (int i = 0; i < nImages; ++i) {
        stamps = vTimeStamp[i];
        stamps_ros = ros::Time(stamps);
        current_time = stamps_ros;
        image_frame_aux = cv::imread(vstrImage[i], CV_LOAD_IMAGE_UNCHANGED);
        cont_frames++;
        semidense_tracker->cont_frames = &cont_frames;
        semidense_tracker->stamps = &stamps;
        semidense_tracker->image_frame = &image_frame_aux;
        semidense_tracker->stamps_ros = &stamps_ros;
//        std::cout << "frame :" << i << std::endl;
        sleep(0.1);

    }
    thread_semidense_mapper.join();
    thread_semidense_tracker.join();

}


bool LoadImages(const string &strPathLeft,const string &strPathTimes,
                vector<string> &vstrImageLeft, vector<double> &vTimeStamps) {
    ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    if (!fTimes) {
        cout << "cannot find timestamp file: " << strPathTimes << endl;
        return false;
    }
    vTimeStamps.reserve(5000);
    vstrImageLeft.reserve(5000);

    string tmp_s;
    getline(fTimes, tmp_s);
    getline(fTimes, tmp_s);
    getline(fTimes, tmp_s);
    while (!fTimes.eof()) {
        string s;
        getline(fTimes, s);
        if (!s.empty()) {
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            string sname;
            ss >> sname;
            vstrImageLeft.push_back(strPathLeft + "/" + sname);
            vTimeStamps.push_back(t / 1e9);
        }
    }
    fTimes.close();

    if (vstrImageLeft.empty()) {
        cout << "No images in left folder!" << endl;
        return false;
    }

    return true;
}


